/*#include <ros/ros.h>
#include "communication/arm_communication.h"
#include "communication/move_communication.h"
int main(int argc, char **argv)
{
    ros::init(argc, argv, "serial_test");
    ros::NodeHandle nh;
    ArmCom demo;
    demo.ArmComOpen("/dev/ttyUSB0");
    demo.tandemMove(1,500,1000);
    sleep(3);
    demo.gotoStart(1);
    sleep(3);
    demo.gotoTerminus(1);
    sleep(3);
    std::cout<<demo.tandemPosition(1)<<std::endl;
    return 0;
}*/


#include <ros/ros.h>
#include <serial/serial.h>
#define LOBOT_SERVO_FRAME_HEADER         85
#define LOBOT_SERVO_MOVE_TIME_WRITE      1
#define LOBOT_SERVO_MOVE_TIME_WAIT_WRITE 7
#define GET_LOW_BYTE(A) (uint8_t)((A))
#define GET_HIGH_BYTE(A) (uint8_t)((A) >> 8)
#define BYTE_TO_HW(A, B) ((((uint16_t)(A)) << 8) | (uint8_t)(B))
serial::Serial ser;

unsigned char LobotCheckSum(unsigned char buf[]){
        unsigned char i;
        uint16_t temp = 0;
        for (i = 2; i < buf[3] + 2; i++) {
            temp += buf[i];
        }
        temp = ~temp;
        i = (unsigned char)temp;
        return i;
    }
void tandemMove(uint8_t id, int16_t position, uint16_t time){
        unsigned char buf[10];
        if(position < 0)
            position = 0;
        if(position > 1000)
            position = 1000;
        buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER;
        buf[2] = id;
        buf[3] = 7;
        buf[4] = LOBOT_SERVO_MOVE_TIME_WRITE;
        buf[5] = GET_LOW_BYTE(position);
        buf[6] = GET_HIGH_BYTE(position);
        buf[7] = GET_LOW_BYTE(time);
        buf[8] = GET_HIGH_BYTE(time);
        buf[9] = LobotCheckSum(buf);
        ser.write(buf,10); 
    }
int main(int argc, char **argv)
{
    ros::init(argc, argv, "serial_test");
    ser.setPort("/dev/ttyUSB0");  //设置端口号
    ser.setBaudrate(115200);  //设置波特率
    serial::Timeout tout = serial::Timeout::simpleTimeout(1000);//设置延时等待 ms
    ser.setTimeout(tout); 
    ser.open(); //打开串
    if(ser.isOpen()){
            std::cout<<"aemPort is open"<<std::endl;
             tandemMove(1,500,1000);
        } 
        else {
            std::cout<<"aemPort is not open"<<std::endl;
        } 
    return 0;
}



